/*
	The Shadow Framework - a complete framework for Real Time Graphics based on OpenGL
    Copyright (C) 2008  Alessandro Martinelli  <alessandro.martinelli@unipv.it>

    This file is part of The Shadow Framework.

    The Shadow Framework is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    The Shadow Framework is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with Shadow Framework.  If not, see <http://www.gnu.org/licenses/>.
*/

package objLoader.math;

public class LinearSystem {

	private static final int PRECISION=1000;
	
	private int dimension;
	private float b[];
	private float sol[];
	private float solV[][];
	private float A[];
	private float errMin=0.001f;
	private int solVDimension=0;

	public LinearSystem(int dimension) {
		super();
		this.dimension = dimension;
		A=new float[dimension*dimension];
		b=new float[dimension];
		sol=new float[dimension];
	}
	
	public float[] getSol() {
		return sol;
	}
	
	public void setSol(int i,float val){
		sol[i]=val;
	}
	
	public void setFromB(){
		for(int i=0;i<dimension;i++){
			sol[i]=b[i];
		}
	}

	public void setb(int i,float val){
		b[i]=val;
	}
	
	public float getb(int i){
		return b[i];
	}
	
	public float getA(int i,int j){
		return A[j*dimension+i];
	}
	
	public void setA(int i,int j,float val){
		A[j*dimension+i]=val;
	}
	
	public void solveSystem(){
		
		float Ap[]=new float[A.length];
		for(int i=0;i<A.length;i++){
			Ap[i]=A[i];
		}
		
		float bp[]=new float[dimension];
		for(int i=0;i<dimension;i++){
			bp[i]=b[i];
		}
		
		int index[]=new int[dimension];
		for(int i=0;i<dimension;i++){
			index[i]=i;
		}
		
		float sols[]=new float[dimension];
		
		float firstPivot=0;
		
		//Eliminazione di Gauss..
		for(int i=0;i<dimension-1;i++){//Per Ogni riga, esclusa l'ultima
			//Trovo l'indice con valore assoluto Maggiore
			int pivotIndex=i;
			int pivotIndexK=i;
			float pivot=Ap[i*dimension+i];
			pivot=pivot>0?pivot:-pivot;
			for(int j=i;j<dimension;j++){
				for(int k=i;k<dimension;k++){
					float tPivot=Ap[j*dimension+k];
					tPivot=tPivot>0?tPivot:-tPivot;
					if(tPivot>pivot){
						pivot=tPivot;
						pivotIndex=j;
						pivotIndexK=k;
					}	
				}
			}
			
			if(i==0)
				firstPivot=pivot;
			
			
			//System.out.println("Pivot "+pivot+" first "+firstPivot);
			if(firstPivot<=(PRECISION*pivot)){
			
				if(pivotIndexK!=i){//Voglio invertire 2 colonne della matrice e 2 valori di index
					for(int j=0;j<dimension;j++){
						//scambio le due colonne (completamente..)
						float temp=Ap[j*dimension+i];
						Ap[j*dimension+i]=Ap[j*dimension+pivotIndexK];
						Ap[j*dimension+pivotIndexK]=temp;
					}
					int temp=index[i];
					index[i]=index[pivotIndexK];
					index[pivotIndexK]=temp;
				}
				
				if(pivotIndex!=i){
					for(int k=0;k<dimension;k++){
						//scambio le due colonne (completamente..)
						float temp=Ap[i*dimension+k];
						Ap[i*dimension+k]=Ap[pivotIndex*dimension+k];
						Ap[pivotIndex*dimension+k]=temp;
					}
					float temp=bp[i];
					bp[i]=bp[pivotIndex];
					bp[pivotIndex]=temp;
				}
				
				float rec=1.0f/(Ap[dimension*i+i]);
				//Bene, ora ho risolto il pivoting ->Eliminazione di Gauss
				for(int j=i+1;j<dimension;j++){//Per ogni riga successiva all'attuale..
					float delta=(Ap[dimension*j+i])*rec;//  a  b  : Devo togliere  la prima riga*(b/a) in modo che b sia 0
					for(int k=i+1;k<dimension;k++){
						Ap[dimension*j+k]-=delta*Ap[dimension*i+k];
					}
					Ap[dimension*j+i]=0;
					//inoltre..
					bp[j]-=delta*bp[i];
				}
			}else{

				System.out.println("Ciao ciao ciao");
				solVDimension=dimension-i;
				
				for(int j=i;j<dimension;j++){
					for(int k=i;k<dimension;k++){
						if(j==k)
							Ap[j*dimension+k]=1;
						else
							Ap[j*dimension+k]=0;
					}
					bp[j]=0;
				}
			
				i=dimension;
			}
		}
		
//		System.out.println(" Ap[dimension*dimension-1] "+Ap[dimension*dimension-1]+" firstPivot "+firstPivot+" "+PRECISION);
		if(firstPivot>(PRECISION*Ap[dimension*dimension-1])){
			//System.out.println("Cai cai cai");
			solVDimension=1;
			Ap[dimension*dimension-1]=1;
			bp[dimension-1]=0;
		}
		
		//Risoluzione..
		for(int i=dimension-1;i>=0;i--){//Dall'ultima riga alla prima, determino sol[i]
			float sum=bp[i];
			for(int j=i+1;j<dimension;j++){
				sum-=Ap[i*dimension+j]*sols[j];
			}
			sols[i]=sum/Ap[i*(dimension+1)];
//			System.out.println(" I "+i+" sum "+sum+" "+Ap[i*dimension+i]+" b[i] "+bp[i]);
		}
		
		//ordinamento delle soluzioni
		for(int i=0;i<dimension;i++){
			this.sol[index[i]]=sols[i];
//			System.out.println("    		Sol[index[i]]  "+this.sol[index[i]]);
//			System.out.println("i "+i+" index "+index[i]+" sols "+sols[index[i]]+" soli "+sols[i]);
		}
		
		solV=new float[solVDimension][];
		
		for(int m=0;m<solVDimension;m++){
			
			int idx=dimension-solVDimension;
			solV[m]=new float[dimension];
			for(int s=0;s<solVDimension;s++){
				if(s==m)
					bp[s+idx]=1;
				else
					bp[s+idx]=0;
			}
			
			for(int i=dimension-1;i>=0;i--){
				float sum=bp[i];
				for(int j=i+1;j<dimension;j++){
					sum-=Ap[i*dimension+j]*sols[j];
				}
				sols[i]=sum/Ap[i*(dimension+1)];
//				System.out.println(" I "+i+" sum "+sum+" "+Ap[i*dimension+i]+" b[i] "+bp[i]);
			}
			
			for(int i=0;i<dimension;i++){
				this.solV[m][index[i]]=sols[i];
//				System.out.println("    		SolV[index[i]]  "+this.solV[m][index[i]]);
			}
			float sum=0;
			for(int i=0;i<dimension;i++){
				sum+=this.solV[m][i]*A[dimension*(idx+m)+i];
				//System.out.println(" solV[m][i] "+this.solV[m][i]+"  A "+A[dimension*(idx+m)+i]);
			}
			//System.out.println("sum "+sum+" "+b[idx+m]);
			float prec=1.0f/PRECISION;
			if(sum<bp[idx+m]-prec || sum>bp[idx+m]+prec){
				//System.out.println("not solvable ");
			}	
		}
		
		
//		System.out.print("\n");
//		for(int i=0;i<dimension;i++){
//			for(int j=0;j<dimension;j++){
//				System.out.print(" "+Ap[i*dimension+j]+"");
//			}
//			System.out.print("\n");
//		}
//		System.out.print("\n");
//		for(int i=0;i<dimension;i++){
//			
//			System.out.print("b   "+b[i]+"");
//			System.out.print("\n");
//		}
	}
	
	public float getErrMin() {
		return errMin;
	}
	
	public float[][] getSolV() {
		return solV;
	}

	public void setSolV(float[][] solV) {
		this.solV = solV;
	}

	public void setErrMin(float errMin) {
		this.errMin = errMin;
	}

	public static void main(String[] args) {
		
		LinearSystem system=new LinearSystem(5);
		
		system.setb(0, 1);
		system.setb(1, -1);
		system.setb(2, 1);
		system.setb(3, -1);
		
		system.setA(0,0, 1);
		system.setA(1,1, 1);
		system.setA(2,2, 1);
		system.setA(3,3, 1);
		system.setA(4,4, 1);
		
		system.setA(1,0, 4);
		system.setA(2,1, 1);
		system.setA(3,2, 1);
		system.setA(4,3, 1);
		system.setA(3,4, 1);
		
		system.solveSystem();
	
		float res[]=system.getSol();
		float resV[][]=system.getSolV();
		
		for(int i=0;i<system.dimension;i++){
			System.out.println(res[i]);
		}
		
		for(int j=0;j<resV.length;j++)
			for(int i=0;i<system.dimension;i++){
				System.out.println(" j "+j+" "+resV[j][i]);
			}
	}
}
